#include "mainwindow.h"
#include "ui_mainwindow.h"

#include <opencv2/opencv.hpp>

using namespace cv;
using namespace std;


void test();

MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);
//    Mat img = imread("/home/android/Opensource/WorkSpace_Qt/build-test-Desktop_Qt_5_9_0_GCC_64bit-Debug/a.jpg");
//    imshow("test", img);
    test();
}

MainWindow::~MainWindow()
{
    delete ui;
}



/** Global Variables */
const int size_1_max = 100;
const int size_2_max = 100;
int size_1 = 50;
int size_2 = 50;

const int cen_1_max = 100;
const int cen_2_max = 100;
int cen_1 = 50;
int cen_2 = 50;

Mat s_intrinsic_matrix;
Mat s_distortion_coeffs;
Mat s_src_img;


void SaveMatrix(const Mat &src, const string &filename) {
    FileStorage fs(filename, FileStorage::WRITE);
    write(fs, "data", src);
    fs.release();
}

void ReadMatrix(Mat &dst, const string &filename) {
     FileStorage fs(filename, FileStorage::READ);
     read(fs["data"], dst);
     fs.release();
}


static void undistortion(const Mat distort_img, const Mat intrinsic_matrix, const Mat distortion_coeffs, Mat &undistortion, float size1, float size2, float cen1, float cen2){
    Mat intrinsic_mat(intrinsic_matrix),new_intrinsic_mat;
    intrinsic_mat.copyTo(new_intrinsic_mat);
    //调节视场大小,乘的系数越小视场越大
    new_intrinsic_mat.at<double>(0,0) *= size1;
    new_intrinsic_mat.at<double>(1,1) *= size2;
    //调节校正图中心，建议置于校正图中心
    new_intrinsic_mat.at<double>(0,2) = cen1 * distort_img.cols;
    new_intrinsic_mat.at<double>(1,2) = cen2 * distort_img.rows;
    fisheye::undistortImage(distort_img, undistortion,intrinsic_matrix,distortion_coeffs,new_intrinsic_mat);
}


static void on_track_change(int, void*){
    Mat undis;
    float cover_size1 = (float)(size_1) / (float)(100);
    float cover_size2 = (float)(size_2) / (float)(100);
    float cover_cen1 = (float)(cen_1) / (float)(100);
    float cover_cen2 = (float)(cen_2) / (float)(100);
    undistortion(s_src_img, s_intrinsic_matrix, s_distortion_coeffs, undis, cover_size1, cover_size2, cover_cen1, cover_cen2);
    imshow("Undist Fish Eye", undis);
}


void test(){
    ReadMatrix(s_intrinsic_matrix, "/home/android/Opensource/WorkSpace_Qt/CalibrateCV/UndistortionCV3/intrinsic_matrix.xml");
    ReadMatrix(s_distortion_coeffs, "/home/android/Opensource/WorkSpace_Qt/CalibrateCV/UndistortionCV3/distortion_coeffs.xml");

    namedWindow("Undist Fish Eye", WINDOW_AUTOSIZE); // Create Window
    s_src_img = imread("/home/android/Opensource/WorkSpace_Qt/CalibrateCV/UndistortionCV3/a.jpg");
    createTrackbar( "size 1", "Undist Fish Eye", &size_1, size_1_max, on_track_change );
    createTrackbar( "size 2", "Undist Fish Eye", &size_2, size_2_max, on_track_change );
    createTrackbar( "cen 1", "Undist Fish Eye", &cen_1, cen_1_max, on_track_change );
    createTrackbar( "cen 2", "Undist Fish Eye", &cen_2, cen_2_max, on_track_change );
    on_track_change(1, NULL);
}
